#include "filter.h"
#include <math.h>


void CalculationCoefficient(FilterType _type,double _frequency,double _gain,double _qfactor,double _sampleRate,FilterCoefficient *coefficient)
{

    const double PI = 3.1415926;
    double Q = _qfactor;
    Q = (Q == 0) ? 1e-9 : Q;
    double gain_abs = pow(10.0, _gain / 40.0);
    double omega = 2.0f * PI * _frequency / _sampleRate;
    double sn = sin(omega);
    double cs = cos(omega);
    double alpha = sn / (2.0 * Q);
    double beta = sqrt(gain_abs + gain_abs);
    double a0,a1,a2,b0,b1,b2;

    switch (_type)
    {
    case Bandpass:
        b0 = alpha;
        b1 = 0;
        b2 = -alpha;
        a0 = 1.0 + alpha;
        a1 = -2.0 * cs;
        a2 = 1.0 - alpha;
        break;
    case Lowpass:
        b0 = (1 - cs) / 2;
        b1 = 1 - cs;
        b2 = (1 - cs) / 2;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case Hightpass:
        b0 = (1 + cs) / 2;
        b1 = -(1 + cs);
        b2 = (1 + cs) / 2;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case Notch:
        b0 = 1;
        b1 = -2 * cs;
        b2 = 1;
        a0 = 1 + alpha;
        a1 = -2 * cs;
        a2 = 1 - alpha;
        break;
    case Peak:
        b0 = 1 + (alpha * gain_abs);
        b1 = -2 * cs;
        b2 = 1 - (alpha * gain_abs);
        a0 = 1 + (alpha / gain_abs);
        a1 = -2 * cs;
        a2 = 1 - (alpha / gain_abs);
        break;
    case Lowshelf:
        b0 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs + beta * sn);
        b1 = 2 * gain_abs * ((gain_abs - 1) - (gain_abs + 1) * cs);
        b2 = gain_abs * ((gain_abs + 1) - (gain_abs - 1) * cs - beta * sn);
        a0 = (gain_abs + 1) + (gain_abs - 1) * cs + beta * sn;
        a1 = -2 * ((gain_abs - 1) + (gain_abs + 1) * cs);
        a2 = (gain_abs + 1) + (gain_abs - 1) * cs - beta * sn;
        break;
    case Highshelf:
        b0 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs + beta * sn);
        b1 = -2 * gain_abs * ((gain_abs - 1) + (gain_abs + 1) * cs);
        b2 = gain_abs * ((gain_abs + 1) + (gain_abs - 1) * cs - beta * sn);
        a0 = (gain_abs + 1) - (gain_abs - 1) * cs + beta * sn;
        a1 = 2 * ((gain_abs - 1) - (gain_abs + 1) * cs);
        a2 = (gain_abs + 1) - (gain_abs - 1) * cs - beta * sn;
        break;
    default:
        b0 = 0;
        b1 = 0;
        b2 = 0;
        a1 = 0;
        a2 = 0;
        a0 = 1;
    break;
    }
    if(coefficient)
    {
        coefficient->b0 = b0 / a0;
        coefficient->b1 = b1 / a0;
        coefficient->b2 = b2 / a0;
        coefficient->a1 = a1 / a0;
        coefficient->a2 = a2 / a0;
    }
}


int GetFrequencyResponseData(double start_freq, double end_freq,double freq_step,double sampleRate,FilterCoefficient *coefficient,double *out_buff)
{
    const double PI = 3.1415926;
    double a1,a2,b0,b1,b2;
    double sf,ef;
    double phi;
    double r;
    double g;
    double dB;
    a1 = coefficient->a1;
    a2 = coefficient->a2;
    b0 = coefficient->b0;
    b1 = coefficient->b1;
    b2 = coefficient->b2;
    sf = start_freq;
    ef = end_freq;
    int cnt = 0;
    if(out_buff == 0)
    {
        return 0;
    }
    // frequency_response_data.clear();
    while(sf <= ef)
    {
        phi = pow((sin(2.0 * PI * sf / (2.0 * sampleRate))), 2.0);
        r = (pow(b0 + b1 + b2, 2.0) - 4.0 * (b0 * b1 + 4.0 * b0 * b2 + b1 * b2) * phi + 16.0 * b0 * b2 * phi * phi) / (pow(1.0 + a1 + a2, 2.0) - 4.0 * (a1 + 4.0 * a2 + a1 * a2) * phi + 16.0 * a2 * phi * phi);
        if(r < 0) {
            r = 0;
        }
        g = sqrt(r);
        dB = 20 * log10(g == 0 ? 1 : g);
        // frequency_response_data.push_back(dB);
        *out_buff++ = dB;
        sf += freq_step;
        cnt ++;
    }
    if(sf != ef)
    {
        sf = ef;
        phi = pow((sin(2.0 * PI * sf / (2.0 * sampleRate))), 2.0);
        r = (pow(b0 + b1 + b2, 2.0) - 4.0 * (b0 * b1 + 4.0 * b0 * b2 + b1 * b2) * phi + 16.0 * b0 * b2 * phi * phi) / (pow(1.0 + a1 + a2, 2.0) - 4.0 * (a1 + 4.0 * a2 + a1 * a2) * phi + 16.0 * a2 * phi * phi);
        if(r < 0) {
            r = 0;
        }
        g = sqrt(r);
        dB = 20 * log10(g == 0 ? 1 : g);
        // frequency_response_data.push_back(dB);
        *out_buff++ = dB;
        cnt ++;
    }
    return cnt;
}
